#include <camdrv.hpp>
#include <chrono>
#include <iomanip>
#include <sstream>

namespace rm_camdrv {
void CamdrvNode::checkMode(const interfaces::msg::Readpack::SharedPtr msg) {
  if (msg->mode == last_mode_) {
    return; // No change in mode, do nothing
  }

  RCLCPP_INFO(this->get_logger(), "SWITCH TO MODE %d", msg->mode);

  try {
    int last_camera_id = mode_params_.at(current_mode_)->camera_id;
    destroyCameraThread(mode_params_.at(current_mode_)->camera_handle);
    switch (msg->mode) {
      case CAMMOD::AIMBOT:
        current_mode_ = "Aimbot";
        break;
      case CAMMOD::SMALL_RUNE:
      case CAMMOD::BIG_RUNE:
        current_mode_ = "Rune";
        break;
      case CAMMOD::OUTPOST:
        current_mode_ = "Outpost";
        break;
      case CAMMOD::BASE:
        current_mode_ = "Base";
        break;
      case CAMMOD::HIT_FARAWAY:
        current_mode_ = "HitFaraway";
        break;
      default:
        RCLCPP_ERROR(this->get_logger(), "UNKNOWN MODE: %d", msg->mode);
    }
    setParams(current_mode_);
    createCameraThread(mode_params_.at(current_mode_)->camera_handle);
    if (last_camera_id != mode_params_.at(current_mode_)->camera_id) {
      callCamInfoUpdater();
    }
    RCLCPP_INFO(this->get_logger(), "Switching to %s camera %d", current_mode_.c_str(), mode_params_.at(current_mode_)->camera_id);
  } catch (...) {
    RCLCPP_ERROR(this->get_logger(), "Error while switching mode: %d", msg->mode);
    return;
  }
  RCLCPP_INFO(this->get_logger(), "-------------------");
  last_mode_ = msg->mode;
}

void CamdrvNode::setParams(const std::string& mode) {
  const auto& params = mode_params_.at(mode);
  this->set_parameter(rclcpp::Parameter("Width", params->width));
  this->set_parameter(rclcpp::Parameter("Height", params->height));
  this->set_parameter(rclcpp::Parameter("ExposureTime", params->exposure_time));
  this->set_parameter(rclcpp::Parameter("Gain", params->gain));
  this->set_parameter(rclcpp::Parameter("BalanceRatioRed", params->balance_ratio_red));
  this->set_parameter(rclcpp::Parameter("BalanceRatioGreen", params->balance_ratio_green));
  this->set_parameter(rclcpp::Parameter("BalanceRatioBlue", params->balance_ratio_blue));
  this->set_parameter(rclcpp::Parameter("AutoWhiteBalance", params->auto_white_balance));  // set AWB mode last
}

void CamdrvNode::callCamInfoUpdater() {
  if (!cam_info_updater_cli_) {
    RCLCPP_ERROR(this->get_logger(), "cam_info_updater_cli_ is not initialized.");
    return;
  }

  if (!cam_info_updater_cli_->wait_for_service(std::chrono::seconds(10))) {
    RCLCPP_ERROR(this->get_logger(), "Camera info updater service not available.");
    return;
  }

  auto request = std::make_shared<rm_interfaces::srv::SetCameraID::Request>();
  request->camera_id = mode_params_.at(current_mode_)->camera_id;

  auto future = cam_info_updater_cli_->async_send_request(request,
    [this](rclcpp::Client<rm_interfaces::srv::SetCameraID>::SharedFuture result) {
      if (result.valid()) {
        if (result.get()->success) {
          RCLCPP_INFO(this->get_logger(), "Camera info updated successfully.");
        } else {
          RCLCPP_WARN(this->get_logger(), "Camera info updater responded with failure: %s", result.get()->message.c_str());
        }
      } else {
        RCLCPP_ERROR(this->get_logger(), "Camera info updater response is invalid.");
      }
    });
}


void CamdrvNode::destroyCameraThread(void* handle) {
  RCLCPP_INFO(this->get_logger(), "Destroying camera thread...");
  
  // Stop capture thread
  if (capture_thread_.joinable()) {
    camera_thread_alive_ = false;
    capture_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Capture thread destroyed.");
  }
  
  // Stop recording thread
  if (recording_thread_.joinable()) {
    recording_thread_alive_ = false;
    // Notify recording thread to exit
    {
      std::lock_guard<std::mutex> lock(queue_mutex_);
      queue_condition_.notify_all();
    }
    recording_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Recording thread destroyed.");
  }
  
  // Release recording resources
  if (video_writer_.isOpened()) {
    video_writer_.release();
    RCLCPP_INFO(this->get_logger(), "Recording stopped.");
  }
  
  // Clear recording queue
  {
    std::lock_guard<std::mutex> lock(queue_mutex_);
    while (!frame_queue_.empty()) {
      frame_queue_.pop();
    }
  }
  
  // Stop camera grabbing
  if (handle) {
    MV_CC_StopGrabbing(handle);
    RCLCPP_INFO(this->get_logger(), "Camera grabbing stopped.");
  }
  
  RCLCPP_INFO(this->get_logger(), "Camera thread destroyed!");
}

void CamdrvNode::createCameraThread(void* handle) {
  // Get camera infomation
  img_info_ = MV_IMAGE_BASIC_INFO();  
  image_msg_ = sensor_msgs::msg::Image();
  MV_CC_GetImageInfo(handle, &img_info_);
  image_msg_.data.reserve(img_info_.nHeightMax * img_info_.nWidthMax * 3);

  // Set camera pixel format to RGB8 directly if preferred
  use_native_rgb8_ = false; // Default to false
  
  if (prefer_native_rgb8_) {
    MVCC_ENUMVALUE pixel_format_info;
    int status = MV_CC_GetPixelFormat(handle, &pixel_format_info);
    
    if (MV_OK == status) {
      // Check if RGB8_Packed is supported
      bool rgb8_supported = false;
      for (unsigned int i = 0; i < pixel_format_info.nSupportedNum; ++i) {
        if (pixel_format_info.nSupportValue[i] == PixelType_Gvsp_RGB8_Packed) {
          rgb8_supported = true;
          break;
        }
      }
      
      if (rgb8_supported) {
        status = MV_CC_SetPixelFormat(handle, PixelType_Gvsp_RGB8_Packed);
        if (MV_OK == status) {
          use_native_rgb8_ = true;
          RCLCPP_INFO(this->get_logger(), "Camera pixel format set to RGB8_Packed successfully");
        } else {
          RCLCPP_WARN(this->get_logger(), "Failed to set pixel format to RGB8_Packed, status: [%x]", status);
        }
      } else {
        RCLCPP_WARN(this->get_logger(), "RGB8_Packed format not supported by camera, will use conversion");
      }
    } else {
      RCLCPP_WARN(this->get_logger(), "Failed to get pixel format info, status: [%x]", status);
    }
  } else {
    RCLCPP_INFO(this->get_logger(), "Native RGB8 disabled by parameter, using bayer conversion");
  }

  // Init convert param (only used if RGB8 not natively supported)
  convert_param_ = MV_CC_PIXEL_CONVERT_PARAM();
  convert_param_.nWidth = img_info_.nWidthValue;
  convert_param_.nHeight = img_info_.nHeightValue;
  convert_param_.enDstPixelType = PixelType_Gvsp_RGB8_Packed;

  // Record image if enabled
  if (record_) {
    try {
      createRecorder();
      // Initialize frame skip counter
      frame_skip_counter_ = 0;
      RCLCPP_INFO(this->get_logger(), "Recording: skip every %d frames (record 1 out of %d frames)", 
                  frame_skip_interval_, frame_skip_interval_);
      
      // Start recording thread
      if (recording_thread_.joinable()) {
        recording_thread_alive_ = false;
        queue_condition_.notify_all();
        recording_thread_.join();
      }
      recording_thread_alive_ = true;
      frames_recorded_ = 0;

      recording_thread_ = std::thread{[this]() -> void {
        while (recording_thread_alive_) {
          std::unique_lock<std::mutex> lock(queue_mutex_);
          queue_condition_.wait(lock, [this] { 
            return !frame_queue_.empty() || !recording_thread_alive_; 
          });
          
          while (!frame_queue_.empty() && recording_thread_alive_) {
            cv::Mat frame = frame_queue_.front();
            frame_queue_.pop();
            lock.unlock();
            
            if (video_writer_.isOpened()) {
              try {
                // Add ROS timestamp to frame
                cv::putText(frame, std::to_string(this->now().seconds()), cv::Point(10, 30), 
                          cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1);
                
                // Add exposure time and gain parameters
                double exposure_time, gain;
                this->get_parameter("ExposureTime", exposure_time);
                this->get_parameter("Gain", gain);
                
                std::stringstream exp_gain_ss;
                exp_gain_ss << std::fixed << std::setprecision(1) << "Exp:" << exposure_time << " Gain:" << gain;
                cv::putText(frame, exp_gain_ss.str(), 
                          cv::Point(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1);
                          
                video_writer_.write(frame);
                
                if (++frames_recorded_ >= max_frames_) {
                  createRecorder();
                  frames_recorded_ = 0;
                }
              } catch (const cv::Exception& e) {
                RCLCPP_ERROR(this->get_logger(), "Failed to write frame: %s", e.what());
              }
            }
            lock.lock();
          }
        }
        
        // Process remaining frames when shutting down
        RCLCPP_INFO(this->get_logger(), "Recording thread shutting down, processing remaining frames...");
        std::unique_lock<std::mutex> lock(queue_mutex_);
        int remaining_frames = frame_queue_.size();
        while (!frame_queue_.empty()) {
          cv::Mat frame = frame_queue_.front();
          frame_queue_.pop();
          if (video_writer_.isOpened()) {
            video_writer_.write(frame);
          }
        }
        RCLCPP_INFO(this->get_logger(), "Recording thread processed %d remaining frames and exited.", remaining_frames);
      }};
    } catch (const cv::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "Failed to open video writer: %s", e.what());
      record_ = false;
    }
  }

  MV_CC_StartGrabbing(handle);

  // Destroy if thread is alive
  if (capture_thread_.joinable()) {
    capture_thread_.join();
  }

  capture_thread_ = std::thread{[this]() -> void {
    auto handle = mode_params_.at(current_mode_)->camera_handle;
    auto& camera_info_msg = *mode_params_.at(current_mode_)->camera_info_msg;
    MV_FRAME_OUT out_frame;

    RCLCPP_INFO(this->get_logger(), "Publishing image!");

    image_msg_.header.frame_id = "camera" + std::to_string(mode_params_.at(current_mode_)->camera_id) + "_optical_frame";
    image_msg_.encoding = "rgb8";

    camera_thread_alive_ = true;
    while (rclcpp::ok() && camera_thread_alive_) {
      nRet = MV_CC_GetImageBuffer(handle, &out_frame, 1000);
      if (MV_OK == nRet) {
        // Check if the frame is already in RGB8 format and we want to use it natively
        bool need_conversion = !(use_native_rgb8_ && out_frame.stFrameInfo.enPixelType == PixelType_Gvsp_RGB8_Packed);
        
        if (need_conversion) {
          // Convert from Bayer or other format to RGB8
          convert_param_.pDstBuffer = image_msg_.data.data();
          convert_param_.nDstBufferSize = image_msg_.data.size();
          convert_param_.pSrcData = out_frame.pBufAddr;
          convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
          convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;

          MV_CC_ConvertPixelType(handle, &convert_param_);
          RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, 
                               "Converting from pixel type [%x] to RGB8", out_frame.stFrameInfo.enPixelType);
        } else {
          // Direct copy RGB8 data (no conversion needed)
          size_t data_size = out_frame.stFrameInfo.nWidth * out_frame.stFrameInfo.nHeight * 3;
          image_msg_.data.resize(data_size);
          memcpy(image_msg_.data.data(), out_frame.pBufAddr, data_size);
          RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 5000, 
                               "Using native RGB8 format directly");
        }

        image_msg_.header.stamp = this->now();
        image_msg_.height = out_frame.stFrameInfo.nHeight;
        image_msg_.width = out_frame.stFrameInfo.nWidth;
        image_msg_.step = out_frame.stFrameInfo.nWidth * 3;
        if (need_conversion) {
          image_msg_.data.resize(image_msg_.width * image_msg_.height * 3);
        }

        if (record_) {
          // Frame rate control - only record every N frames
          if (++frame_skip_counter_ >= frame_skip_interval_) {
            frame_skip_counter_ = 0; // Reset counter
            
            try {
              // Convert RGB to BGR for recording (non-blocking)
              cv::Mat frame(image_msg_.height, image_msg_.width, CV_8UC3);
              // Direct RGB to BGR conversion without cv_bridge
              const uint8_t* src_data = image_msg_.data.data();
              uint8_t* dst_data = frame.data;
              size_t pixel_count = image_msg_.height * image_msg_.width;
              
              for (size_t i = 0; i < pixel_count; ++i) {
                dst_data[i * 3 + 0] = src_data[i * 3 + 2]; // B
                dst_data[i * 3 + 1] = src_data[i * 3 + 1]; // G  
                dst_data[i * 3 + 2] = src_data[i * 3 + 0]; // R
              }
              
              // Add frame to queue for async processing
              {
                std::lock_guard<std::mutex> lock(queue_mutex_);
                if (frame_queue_.size() < 10) { // Limit queue size to prevent memory issues
                  frame_queue_.push(frame.clone());
                  queue_condition_.notify_one();
                } else {
                  // Drop frame if queue is full
                  static int drop_count = 0;
                  if (++drop_count % 30 == 0) { // Log every 30 dropped frames
                    RCLCPP_WARN(this->get_logger(), "Recording queue full, dropped %d frames", drop_count);
                  }
                }
              }
            } catch (const cv::Exception& e) {
              RCLCPP_ERROR(this->get_logger(), "Failed to convert image for recording: %s", e.what());
              record_ = false;
            }
          }
        }

        camera_info_msg.header = image_msg_.header;

        camera_pub_.publish(image_msg_, camera_info_msg);

        MV_CC_FreeImageBuffer(handle, &out_frame);
        fail_conut_ = 0;
      } else {
        RCLCPP_WARN(this->get_logger(), "Get buffer failed! nRet: [%x]", nRet);
        MV_CC_StopGrabbing(handle);
        MV_CC_StartGrabbing(handle);
        fail_conut_++;
      }

      if (fail_conut_ > 5) {
        RCLCPP_FATAL(this->get_logger(), "Camera failed!");
        camera_thread_alive_ = false; // Ensure thread can exit
        // Use async way to call shutdown, avoid blocking
        std::thread([]() {
          std::this_thread::sleep_for(std::chrono::milliseconds(100));
          rclcpp::shutdown();
        }).detach();
        break; // Exit thread loop
      }
    }
    RCLCPP_INFO(this->get_logger(), "Capture thread exiting...");
  }};
}

void CamdrvNode::createRecorder() {
  if (video_writer_.isOpened()) {
    video_writer_.release();
    RCLCPP_INFO(this->get_logger(), "Recording stopped.");
  }

  // Get timestamp
  auto now = std::chrono::system_clock::now();
  std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
  std::tm* now_tm = std::localtime(&now_time_t);
  std::stringstream ss;
  ss << std::put_time(now_tm, "%Y-%m-%d_%H-%M-%S");

  std::string prefix = record_path_ + current_mode_ + "/";
  std::string filename = ss.str() + ".avi";

  // Ensure the directory exists
  std::filesystem::create_directories(prefix);

  video_writer_.open(prefix + filename, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'),
                    fps_, cv::Size(img_info_.nWidthValue, img_info_.nHeightValue), true);
  RCLCPP_INFO(this->get_logger(), "Recording started (skip interval: %d).", frame_skip_interval_);
}

void CamdrvNode::createParamSweepThread() {
  param_sweep_thread_alive_ = true;
  
  param_sweep_thread_ = std::thread{[this]() -> void {
    RCLCPP_INFO(this->get_logger(), "Parameter sweep thread started!");
    
    exposure_time_sweep_.reset();
    gain_sweep_.reset();
    
    while (param_sweep_thread_alive_ && rclcpp::ok()) {
      double current_exposure = exposure_time_sweep_.current;
      double current_gain = gain_sweep_.current;
      
      RCLCPP_INFO(this->get_logger(), "Setting parameters: Exposure=%.1f, Gain=%.1f", 
                  current_exposure, current_gain);
      
      // Use set_parameter to set parameters
      std::vector<rclcpp::Parameter> params;
      params.push_back(rclcpp::Parameter("ExposureTime", current_exposure));
      params.push_back(rclcpp::Parameter("Gain", current_gain));
      
      this->set_parameters(params);

      if (exposure_time_sweep_.next()) {
        gain_sweep_.next();
      }
      
      // Wait for specified interval time, but can be interrupted
      auto sleep_duration = std::chrono::milliseconds(static_cast<int>(sweep_interval_));
      auto sleep_start = std::chrono::steady_clock::now();
      while (param_sweep_thread_alive_ && rclcpp::ok() && 
             (std::chrono::steady_clock::now() - sleep_start) < sleep_duration) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
      }
    }
    
    RCLCPP_INFO(this->get_logger(), "Parameter sweep thread stopped!");
  }};
  
  RCLCPP_INFO(this->get_logger(), "Parameter sweep thread created!");
}

} // namespace rm_camdrv